home *** CD-ROM | disk | FTP | other *** search
/ SGI Developer Toolbox 6.1 / SGI Developer Toolbox 6.1 - Disc 4.iso / src / haeberli / libgutil / vect.c < prev    next >
C/C++ Source or Header  |  1994-08-01  |  8KB  |  457 lines
  1. /*
  2.  * Copyright 1991, 1992, 1993, 1994, Silicon Graphics, Inc.
  3.  * All Rights Reserved.
  4.  *
  5.  * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Silicon Graphics, Inc.;
  6.  * the contents of this file may not be disclosed to third parties, copied or
  7.  * duplicated in any form, in whole or in part, without the prior written
  8.  * permission of Silicon Graphics, Inc.
  9.  *
  10.  * RESTRICTED RIGHTS LEGEND:
  11.  * Use, duplication or disclosure by the Government is subject to restrictions
  12.  * as set forth in subdivision (c)(1)(ii) of the Rights in Technical Data
  13.  * and Computer Software clause at DFARS 252.227-7013, and/or in similar or
  14.  * successor clauses in the FAR, DOD or NASA FAR Supplement. Unpublished -
  15.  * rights reserved under the Copyright Laws of the United States.
  16.  */
  17. /*
  18.  *     vect -
  19.  *        Various functions to support operations on vectors.
  20.  *
  21.  *    David M. Ciemiewicz, Mark Grossman, Henry Moreton, and Paul Haeberli 
  22.  *
  23.  */
  24. #include "vect.h"
  25.  
  26. vect *vnew()
  27. {
  28.     vect *v;
  29.  
  30.     v = (vect *)mymalloc(sizeof(vect));
  31.     return v;
  32. }
  33.  
  34. vect *vclone(v)
  35. vect *v;
  36. {
  37.     vect *c;
  38.  
  39.     c = vnew();
  40.     *c = *v;
  41.     return c;
  42. }
  43.  
  44. vcopy(v1,v2)
  45. vect *v1, *v2;
  46. {
  47.     *v2 = *v1;
  48. }
  49.  
  50. vprint(v)
  51. vect *v;
  52. {
  53.     printf("x: %f y: %f z: %f\n",v->x,v->y,v->z);
  54. }
  55.  
  56. dvprint(v)
  57. dvect *v;
  58. {
  59.     printf("x: %f y: %f z: %f\n",v->x,v->y,v->z);
  60. }
  61.  
  62. vprint4(v)
  63. vect *v;
  64. {
  65.     printf("x: %f y: %f z: %f w: %f\n",v->x,v->y,v->z,v->w);
  66. }
  67.  
  68. vset(v,x,y,z)
  69. vect *v;
  70. float x, y, z;
  71. {
  72.     v->x = x;
  73.     v->y = y;
  74.     v->z = z;
  75. }
  76.  
  77. vzero(v)
  78. vect *v;
  79. {
  80.     v->x = 0.0;
  81.     v->y = 0.0;
  82.     v->z = 0.0;
  83.     v->w = 0.0;
  84. }
  85.  
  86. vone(v)
  87. vect *v;
  88. {
  89.     v->x = 1.0;
  90.     v->y = 1.0;
  91.     v->z = 1.0;
  92.     v->w = 1.0;
  93. }
  94.  
  95. vset4(v,x,y,z,w)
  96. vect *v;
  97. float x, y, z, w;
  98. {
  99.     v->x = x;
  100.     v->y = y;
  101.     v->z = z;
  102.     v->w = w;
  103. }
  104.  
  105. vnormal(v)
  106. vect *v;
  107. {
  108.     float len;
  109.  
  110.     len = vlength(v);
  111.     if(len>0.0)
  112.     vscale(v,1.0/len);
  113.     else
  114.     vzero(v);
  115. }
  116.  
  117. float vlength(v) 
  118. vect *v;
  119. {
  120.     return sqrt(v->x*v->x + v->y*v->y + v->z*v->z);
  121.  
  122. }
  123.  
  124. vscale(v,mul)
  125. vect *v;
  126. float mul;
  127. {
  128.     v->x *= mul;
  129.     v->y *= mul;
  130.     v->z *= mul;
  131. }
  132.  
  133. vmult(src1,src2,dst)
  134. vect *src1, *src2, *dst;
  135. {
  136.     dst->x = src1->x * src2->x;
  137.     dst->y = src1->y * src2->y;
  138.     dst->z = src1->z * src2->z;
  139. }
  140.  
  141. vadd(src1,src2,dst)
  142. vect *src1, *src2, *dst;
  143. {
  144.     dst->x = src1->x + src2->x;
  145.     dst->y = src1->y + src2->y;
  146.     dst->z = src1->z + src2->z;
  147. }
  148.  
  149. vsub(src1,src2,dst)
  150. vect *src1, *src2, *dst;
  151. {
  152.     dst->x = src1->x - src2->x;
  153.     dst->y = src1->y - src2->y;
  154.     dst->z = src1->z - src2->z;
  155. }
  156.  
  157. vhalf(v1,v2,half)
  158. vect *v1, *v2, *half;
  159. {
  160.     float len;
  161.  
  162.     vadd(v2,v1,half);
  163.     len = vlength(half);
  164.     if(len>0.0001)
  165.     vscale(half,1.0/len);
  166.     else
  167.     *half = *v1;
  168. }
  169.  
  170. float vdot(v1,v2) 
  171. vect *v1, *v2;
  172. {
  173.     return v1->x*v2->x + v1->y*v2->y + v1->z*v2->z;
  174. }
  175.  
  176. double dvdot(v1,v2) 
  177. dvect *v1, *v2;
  178. {
  179.     return v1->x*v2->x + v1->y*v2->y + v1->z*v2->z;
  180. }
  181.  
  182. vcross(v1, v2, cross)
  183. vect *v1, *v2, *cross;
  184. {
  185.     vect temp;
  186.  
  187.     temp.x = (v1->y * v2->z) - (v1->z * v2->y);
  188.     temp.y = (v1->z * v2->x) - (v1->x * v2->z);
  189.     temp.z = (v1->x * v2->y) - (v1->y * v2->x);
  190.     *cross = temp;
  191. }
  192.  
  193. vplane(normal, point, plane)
  194. vect *normal, *point;
  195. vect *plane;
  196. {
  197.     plane->x = normal->x;
  198.     plane->y = normal->y;
  199.     plane->z = normal->z;
  200.     plane->w = - vdot(normal, point);
  201. }
  202.  
  203. dvplane(normal, point, plane)
  204. dvect *normal, *point;
  205. dvect *plane;
  206. {
  207.     plane->x = normal->x;
  208.     plane->y = normal->y;
  209.     plane->z = normal->z;
  210.     plane->w = - dvdot(normal, point);
  211. }
  212.  
  213. vdirection(v1, dir)
  214. vect *v1, *dir;
  215. {
  216.     *dir = *v1;
  217.     vnormal(dir);
  218. }
  219.  
  220. makeplane(p1, p2, p3, v)
  221. vect *p1, *p2, *p3, *v;
  222. {
  223.     vect a, b;
  224.  
  225.     vsub(p1,p2,&a);
  226.     vsub(p3,p2,&b);
  227.     vcross(&a,&b,&b);
  228.     vnormal(&b);
  229.     vplane(&b,p1,v);
  230. }
  231.  
  232. vreflect(in,mirror,out)
  233. vect *in, *mirror, *out;
  234. {
  235.     vect temp;
  236.  
  237.     temp = *mirror;
  238.     vscale(&temp,vdot(mirror,in));
  239.     vsub(&temp,in,out);
  240.     vadd(&temp,out,out);
  241. }
  242.  
  243. vmultmatrix(m1,m2,prod)
  244. float m1[4][4], m2[4][4], prod[4][4];
  245. {
  246.     int row, col;
  247.     float temp[4][4];
  248.  
  249.     for(row=0 ; row<4 ; row++) 
  250.         for(col=0 ; col<4 ; col++)
  251.             temp[row][col] = m1[row][0] * m2[0][col]
  252.                            + m1[row][1] * m2[1][col]
  253.                            + m1[row][2] * m2[2][col]
  254.                            + m1[row][3] * m2[3][col];
  255.     for(row=0 ; row<4 ; row++) 
  256.         for(col=0 ; col<4 ; col++)
  257.         prod[row][col] = temp[row][col];
  258. }
  259.  
  260. vtransform(v,mat,vt)
  261. vect *v;
  262. float mat[4][4];
  263. vect *vt;
  264. {
  265.     vect t;
  266.  
  267.     t.x = v->x*mat[0][0] + v->y*mat[1][0] + v->z*mat[2][0] + v->w*mat[3][0];
  268.     t.y = v->x*mat[0][1] + v->y*mat[1][1] + v->z*mat[2][1] + v->w*mat[3][1];
  269.     t.z = v->x*mat[0][2] + v->y*mat[1][2] + v->z*mat[2][2] + v->w*mat[3][2];
  270.     t.w = v->x*mat[0][3] + v->y*mat[1][3] + v->z*mat[2][3] + v->w*mat[3][3];
  271.     *vt = t;
  272. }
  273.  
  274. vlerp(v0,v1,v,p)
  275. vect *v0, *v1, *v;
  276. float p;
  277. {
  278.     v->x = flerp(v0->x,v1->x,p);
  279.     v->y = flerp(v0->y,v1->y,p);
  280.     v->z = flerp(v0->z,v1->z,p);
  281.     v->w = flerp(v0->w,v1->w,p);
  282. }
  283.  
  284. dvlerp(v0,v1,v,p)
  285. dvect *v0, *v1, *v;
  286. float p;
  287. {
  288.     v->x = dlerp(v0->x,v1->x,p);
  289.     v->y = dlerp(v0->y,v1->y,p);
  290.     v->z = dlerp(v0->z,v1->z,p);
  291.     v->w = dlerp(v0->w,v1->w,p);
  292. }
  293.  
  294. float flerp(f0,f1,p)
  295. float f0, f1, p; 
  296. {
  297.     return ((f0*(1.0-p))+(f1*p));
  298. }
  299.  
  300. double dlerp(f0,f1,p)
  301. double f0, f1, p; 
  302. {
  303.     return ((f0*(1.0-p))+(f1*p));
  304. }
  305.  
  306. lerp(i0,i1,p)
  307. int i0, i1;
  308. float p; 
  309. {
  310.     return ((i0*(1.0-p))+(i1*p));
  311. }
  312.  
  313. vclamp(v)
  314. vect *v;
  315. {
  316.     if(v->x<0.0)
  317.     v->x = 0.0;
  318.     else if(v->x>1.0)
  319.     v->x = 1.0;
  320.     if(v->y<0.0)
  321.     v->y = 0.0;
  322.     else if(v->y>1.0)
  323.     v->y = 1.0;
  324.     if(v->z<0.0)
  325.     v->z = 0.0;
  326.     else if(v->z>1.0)
  327.     v->z = 1.0;
  328. }
  329.  
  330. float frand();
  331.  
  332. vperturb(v,p,mag)
  333. vect *v, *p;
  334. float mag;
  335. {
  336.     int dir;
  337.     vect t;
  338.     float m;
  339.  
  340.     do  {
  341.     p->x = v->x + mag*(frand()-0.5);
  342.     p->y = v->y + mag*(frand()-0.5);
  343.     p->z = v->z + mag*(frand()-0.5);
  344.     m = vlength(p);
  345.     } while (m<0.01);
  346.     p->x /= m;
  347.     p->y /= m;
  348.     p->z /= m;
  349. }
  350.  
  351. int trinormal(p00,p01,p10,n,tol)
  352. vect *p00,*p01,*p10,*n;
  353. float tol;
  354. {
  355.     double tx, ty, tz;
  356.     double Xj, Yj, Zj;
  357.     double Xi, Yi, Zi;
  358.     double mag;
  359.  
  360.     Xj = p10->x;
  361.     Yj = p10->y;
  362.     Zj = p10->z;
  363.     tx = ty = tz = 0.0;
  364.  
  365.     Xi = Xj; Xj = p00->x;
  366.     Yi = Yj; Yj = p00->y;
  367.     Zi = Zj; Zj = p00->z;
  368.     tx += (Yi - Yj) * (Zi + Zj);
  369.     ty += (Zi - Zj) * (Xi + Xj);
  370.     tz += (Xi - Xj) * (Yi + Yj);
  371.     
  372.     Xi = Xj; Xj = p01->x;
  373.     Yi = Yj; Yj = p01->y;
  374.     Zi = Zj; Zj = p01->z;
  375.     tx += (Yi - Yj) * (Zi + Zj);
  376.     ty += (Zi - Zj) * (Xi + Xj);
  377.     tz += (Xi - Xj) * (Yi + Yj);
  378.     
  379.     Xi = Xj; Xj = p10->x;
  380.     Yi = Yj; Yj = p10->y;
  381.     Zi = Zj; Zj = p10->z;
  382.     tx += (Yi - Yj) * (Zi + Zj);
  383.     ty += (Zi - Zj) * (Xi + Xj);
  384.     tz += (Xi - Xj) * (Yi + Yj);
  385.  
  386.     mag = sqrt(tx*tx + ty*ty + tz*tz);
  387.     if( mag < tol) {
  388.     n->x = 0.0;
  389.     n->y = 0.0;
  390.     n->z = 0.0;
  391.     return 0;
  392.     } else {
  393.     n->x = tx/mag;
  394.     n->y = ty/mag;
  395.     n->z = tz/mag;
  396.     return 1;
  397.     }
  398. }
  399.  
  400. int dtrinormal(p00,p01,p10,n,tol)
  401. dvect *p00,*p01,*p10,*n;
  402. double tol;
  403. {
  404.     double tx, ty, tz;
  405.     double Xj, Yj, Zj;
  406.     double Xi, Yi, Zi;
  407.     double mag;
  408.  
  409.     Xj = p10->x;
  410.     Yj = p10->y;
  411.     Zj = p10->z;
  412.     tx = ty = tz = 0.0;
  413.  
  414.     Xi = Xj; Xj = p00->x;
  415.     Yi = Yj; Yj = p00->y;
  416.     Zi = Zj; Zj = p00->z;
  417.     tx += (Yi - Yj) * (Zi + Zj);
  418.     ty += (Zi - Zj) * (Xi + Xj);
  419.     tz += (Xi - Xj) * (Yi + Yj);
  420.     
  421.     Xi = Xj; Xj = p01->x;
  422.     Yi = Yj; Yj = p01->y;
  423.     Zi = Zj; Zj = p01->z;
  424.     tx += (Yi - Yj) * (Zi + Zj);
  425.     ty += (Zi - Zj) * (Xi + Xj);
  426.     tz += (Xi - Xj) * (Yi + Yj);
  427.     
  428.     Xi = Xj; Xj = p10->x;
  429.     Yi = Yj; Yj = p10->y;
  430.     Zi = Zj; Zj = p10->z;
  431.     tx += (Yi - Yj) * (Zi + Zj);
  432.     ty += (Zi - Zj) * (Xi + Xj);
  433.     tz += (Xi - Xj) * (Yi + Yj);
  434.  
  435.     mag = sqrt(tx*tx + ty*ty + tz*tz);
  436.     if( mag < tol) {
  437.     n->x = 0.0;
  438.     n->y = 0.0;
  439.     n->z = 0.0;
  440.     return 0;
  441.     } else {
  442.     n->x = tx/mag;
  443.     n->y = ty/mag;
  444.     n->z = tz/mag;
  445.     return 1;
  446.     }
  447. }
  448.  
  449. vrand(v)
  450. vect *v;
  451. {
  452.     v->x = (frand()-0.5);
  453.     v->y = (frand()-0.5);
  454.     v->z = (frand()-0.5);
  455.     v->w = 0.0;
  456. }
  457.